#include <rclcpp/rclcpp.hpp>
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include <sensor_msgs/msg/imu.hpp>
#include "std_msgs/msg/float32.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include <mutex>
#include <cmath> // 包含cos和sin函数的头文件
#include "desired_cordinate.h"
#include "rpy_filter.h"
#include "rpy_filter_initialize.h"

// float d = 0.07, b = 0.098;//履带参数
float d = 0.085, b = 0.128;
float vLo = 0.0, vRo = 0.0, vLd = 0.0, vRd = 0.0;
double vo = 0.0, wo = 0.0, vd = 0.0, wd = 0.0;

int i = 0, ik = 10, ij=0;
double roll, pitch, yaw;
double roll_in, pitch_in, yaw_in;
double roll_f, pitch_f, yaw_f;
double R=0.1,Q=0.01;

double xo = 0.0, yo = 0.0, zo = 0.0;
double wxo = 0.0, wyo = 0.0, wzo = 0.0;
double xd = 0.0, yd = 0.0, wzd = 0.0;

float R2, R5, R9;
float ekf_pr;

class PosDataV2 : public rclcpp::Node
{
private:
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
    // 声名定时器指针
    rclcpp::TimerBase::SharedPtr timer_;
    // 声明时间
    rclcpp::Time last_time_;
    tf2::Quaternion initial_orientation;
    tf2::Quaternion q_adjusted;
    tf2::Quaternion q_kalman;
    tf2::Vector3 obs_linear_dis;
    tf2::Vector3 obs_angular_dis;
    tf2::Matrix3x3 m_pre;
    std::mutex mutex_;

    void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        // std::lock_guard<std::mutex> lock(mutex_);
        // 获取IMU的旋转四元数
        tf2::Quaternion q(msg->orientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w);
        if (i <= ik)
        {
            if(i>4)
            {
                // 将四元数转换为3x3矩阵
                tf2::Matrix3x3 m(q);
                m.getRPY(roll, pitch, yaw);
                roll_in = roll_in + roll;
                pitch_in = pitch_in + pitch;
                yaw_in = yaw_in + yaw;
                ij=ij+1;
                if (i == ik)
                {
                    roll_in = roll_in / (double)ij;
                    pitch_in = pitch_in /(double)ij;
                    yaw_in = yaw_in / (double)ij;
                    initial_orientation.setRPY(-roll_in, -pitch_in, -yaw_in - M_PI / 2);
                    tf2::Matrix3x3 m_init(initial_orientation);
                    m_pre=m_init;
                    RCLCPP_INFO(this->get_logger(), "得到初始补偿角: Roll: %f, Pitch: %f, Yaw: %f", roll_in, pitch_in, yaw_in);
                }
            }
            i = i + 1;
        }
        else
        {
            // 计算调整后的四元数
            q_adjusted = initial_orientation * q;
            wo = msg->angular_velocity.z;
        }
    }
    // 声明话题发布者
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr command_publish_pos;
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr command_publish_rpy;
    void timer_callback()
    {
        std::lock_guard<std::mutex> lock(mutex_);
        // 获取当前时间戳
        rclcpp::Time current_time = rclcpp::Clock().now();
        // 计算当前时间与上一次时间戳的差值
        rclcpp::Duration duration = current_time - last_time_;
        // 将时间间隔转换为秒
        double dt = duration.seconds();
        RCLCPP_INFO(this->get_logger(), "时间间隔dt=%f", dt);
        // 更新上一次的时间戳为当前时间
        last_time_ = current_time;
        if (i > ik)
        {
            // 打印调整后的四元数的RPY角度
            tf2::Matrix3x3 m(q_adjusted);
            m.getRPY(roll, pitch, yaw);
            RCLCPP_INFO(this->get_logger(), "当前RPY: roll: %f, pitch: %f, yaw: %f", roll, pitch, yaw);
            if (std::isnan(roll)||std::isnan(pitch)||std::isnan(yaw))
            {
                RCLCPP_INFO(this->get_logger(), "na值, 不往下执行");
            }
            else
            {
                rpy_filter(roll,pitch,yaw,R,Q,&roll_f,&pitch_f,&yaw_f);
                q_kalman.setRPY(roll_f,pitch_f,yaw_f);
                tf2::Matrix3x3 m_kalman(q_kalman);
                std_msgs::msg::Float32MultiArray rpy_data;
                rpy_data.data.resize(3);
                rpy_data.data[0] = (float)roll_f;
                rpy_data.data[1] = (float)pitch_f;
                rpy_data.data[2] = (float)yaw_f;
                command_publish_rpy->publish(rpy_data);
                RCLCPP_INFO(this->get_logger(), "滤波后rpy, roll_f=%f, pitch_f=%f, yaw_f=%f",roll_f,pitch_f,yaw_f);
                //
                RCLCPP_INFO(this->get_logger(), "测得速度, vL=%f, vR=%f",vLo,vRo);
                vo = (vLo + vRo) / 2.0;
                wo=(vRo-vLo)/(2.0*b);
                // 创建线速度向量(vx, vy, vz)
                tf2::Vector3 vo_vec(0.0, vo, 0.0);
                // 创建角速度向量(wx, wy, wz)
                tf2::Vector3 wo_vec(0.0, 0.0, wo);
                // 将旋转矩阵与线速度向量相乘，得到变换后的线速度向量
                tf2::Vector3 vo_transformed = m_pre * vo_vec;
                // 计算相对于世界坐标系的角速度
                tf2::Vector3 wo_transformed = m_pre * wo_vec;
                // 世界角速度
                RCLCPP_INFO(this->get_logger(), "世界坐标系角速度 wx: %f, wy: %f, wz: %f", wo_transformed[0], wo_transformed[1], wo_transformed[2]);
                // 世界线速度
                RCLCPP_INFO(this->get_logger(), "世界坐标系线速度 vx: %f, vy: %f, vz: %f", vo_transformed[0], vo_transformed[1], vo_transformed[2]);
                //
                /**期望位移计算******************************************************************************/
                vd = (vRd + vLd) / 2.0;
                wd = (vRd - vLd) / (2.0 * b);
                desired_cordinate(xo, yo, wzo, vd, wd, 0.1, &xd, &yd, &wzd);
                RCLCPP_INFO(this->get_logger(), "期望位置 xd: %f, yd: %f, wzd: %f", vd, wd, wzd);
                //
                /**实际位移计算******************************************************************************/
                //
                double vx_tr = static_cast<double>(vo_transformed[0]);
                double vy_tr = static_cast<double>(vo_transformed[1]);
                double vz_tr = static_cast<double>(vo_transformed[2]);
                if (abs(vx_tr) >= 0.0001)
                {
                    xo = xo + vx_tr * dt;
                }
                else
                {
                    xo = xo;
                }
                if (abs(vy_tr) >= 0.0001)
                {
                    yo = yo + vy_tr * dt;
                }
                else
                {
                    yo = yo;
                }
                if (abs(vz_tr) >= 0.0001)
                {
                    zo = zo + vz_tr * dt;
                }
                else
                {
                    zo = zo;
                }
                wxo = static_cast<double>(roll_f);
                wyo = static_cast<double>(pitch_f);
                wzo = static_cast<double>(yaw_f + M_PI/2.0);
                // double wx_tr = static_cast<double>(wo_transformed[0]);
                // double wy_tr = static_cast<double>(wo_transformed[1]);
                // double wz_tr = static_cast<double>(wo_transformed[2]);
                // if (abs(wx_tr) >= 0.0001)
                // {
                //     wxo = wxo + wx_tr * dt;
                // }
                // else
                // {
                //     wxo = wxo;
                // }
                // if (abs(wy_tr) >= 0.0001)
                // {
                //     wyo = wyo + wy_tr * dt;
                // }
                // else
                // {
                //     wyo = wyo;
                // }
                // if (abs(wz_tr) >= 0.0001)
                // {
                //     wzo = wzo + wz_tr * dt;
                // }
                // else
                // {
                //     wzo = wzo;
                // }
                RCLCPP_INFO(this->get_logger(), "观测位置 xo: %f, yo: %f, wzo: %f", xo, yo, wzo);
                //
                R2 = m_kalman[0].y();
                R5 = m_kalman[1].y();
                R9 = m_kalman[2].z();
                //
                std_msgs::msg::Float32MultiArray pos_vel_data;
                pos_vel_data.data.resize(10);
                pos_vel_data.data[0] = (float)xo;
                pos_vel_data.data[1] = (float)yo;
                pos_vel_data.data[2] = (float)wzo;
                pos_vel_data.data[3] = (float)xd;
                pos_vel_data.data[4] = (float)yd;
                pos_vel_data.data[5] = (float)wzd;
                pos_vel_data.data[6] = R2;
                pos_vel_data.data[7] = R5;
                pos_vel_data.data[8] = R9;
                if(abs(vLo)>0.0001||abs(vRo)>0.0001)
                {
                    ekf_pr=1.0;
                }
                else
                {
                    ekf_pr=0.0;
                }
                pos_vel_data.data[9] =ekf_pr;
                command_publish_pos->publish(pos_vel_data);
                m_pre=m_kalman;
            }
        }
    }
    // 声明一个订阅者L
    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr command_subscribe_L;
    // 声明一个订阅者R
    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr command_subscribe_R;
    // 收到话题数据的回调函数L
    void vL_callback(const std_msgs::msg::Float32::SharedPtr msg)
    {
        // std::lock_guard<std::mutex> lock(mutex_);
        float rpmL = msg->data;
        vLo = rpmL * 2 * M_PI / 60.0 * (d / 2.0);
    }
    // 收到话题数据的回调函数R
    void vR_callback(const std_msgs::msg::Float32::SharedPtr msg)
    {
        // std::lock_guard<std::mutex> lock(mutex_);
        float rpmR = msg->data;
        vRo = rpmR * 2 * M_PI / 60.0 * (d / 2.0);
    }
    // 声明一个订阅者vel_g
    rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr command_subscribe_vel;
    void velg_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
    {
        // std::lock_guard<std::mutex> lock(mutex_);
        vRd = msg->data[2];
        vLd = msg->data[3];
    }

public:
    PosDataV2(std::string name) : Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "%s节点已经启动.", name.c_str());
        imu_sub_ = this->create_subscription<sensor_msgs::msg::Imu>(
            "imu/data_raw", 10,
            std::bind(&PosDataV2::imuCallback, this, std::placeholders::_1));
        // 创建定时器，100ms为周期，定时发布
        timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&PosDataV2::timer_callback, this));
        // 创建一个订阅者L订阅话题
        command_subscribe_L = this->create_subscription<std_msgs::msg::Float32>(
            "esp_pro_l", 10, std::bind(&PosDataV2::vL_callback, this, std::placeholders::_1));
        // 创建一个订阅者R订阅话题
        command_subscribe_R = this->create_subscription<std_msgs::msg::Float32>(
            "esp_pro_r", 10, std::bind(&PosDataV2::vR_callback, this, std::placeholders::_1));
        // 初始化上一次时间戳为当前时间
        last_time_ = rclcpp::Clock().now();
        // 创建vel_g订阅者
        command_subscribe_vel = this->create_subscription<std_msgs::msg::Float32MultiArray>(
            "vel_g", 10, std::bind(&PosDataV2::velg_callback, this, std::placeholders::_1));
        // 创建发布速度数组
        command_publish_pos = this->create_publisher<std_msgs::msg::Float32MultiArray>("pos_obs_des", 10);
        //发布rpy
        command_publish_rpy = this->create_publisher<std_msgs::msg::Float32MultiArray>("in_rpy",10);
    }
};

int main(int argc, char **argv)
{
    static bool initialized = false;
    if (!initialized) {
        rpy_filter_initialize();
        initialized = true;
    }
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<PosDataV2>("pos_data_v2");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}